import time
from Sentry3 import *
from mpython import *
from machine import UART


uart1 = UART(1, baudrate=57600, tx=Pin.P14, rx=Pin.P13)

Sentry3 = Sentry3(0x60)

brightness=9

# 自定义函数
def UART():
  global my_variable, i, num, flag, param_set
  Sentry3.begin(uart1)
def I2C():
  global my_variable, i, num, flag, param_set
  Sentry3.begin(i2c)
def Result_Display():
  global my_variable, i, num, flag, param_set
  # Sentry3不主动返回检测识别结果，需要主控板发送指令进行读取。读取的流程：首先读取识别结果的数量，接收到指令后，Sentry3会刷新结果数据，如果结果数量不为零，那么主控再发送指令读取结果的相关信息。请务必按此流程构建程序。
  num = (Sentry3.GetValue(sentry3_vision_e.kVisionAprilTag, sentry_obj_info_e.kStatus))
  i = 1
  for index in range(num):
    if (10 > i):
      oled.DispChar((str("Tag #") + str(((str(i))[0:1]))), 0, 16, 1)
    else:
      oled.DispChar((str("Tag #") + str(((str(i))[0:2]))), 0, 16, 1)
    oled.DispChar((str("l=") + str((Sentry3.GetValue(sentry3_vision_e.kVisionAprilTag,sentry_obj_info_e.kLabel,i)))), 64, 16, 1)
    oled.DispChar((str("x=") + str((Sentry3.GetValue(sentry3_vision_e.kVisionAprilTag,sentry_obj_info_e.kXValue,i)))), 0, 32, 1)
    oled.DispChar((str("y=") + str((Sentry3.GetValue(sentry3_vision_e.kVisionAprilTag,sentry_obj_info_e.kYValue,i)))), 64, 32, 1)
    oled.DispChar((str("w=") + str((Sentry3.GetValue(sentry3_vision_e.kVisionAprilTag,sentry_obj_info_e.kWidthValue,i)))), 0, 48, 1)
    oled.DispChar((str("h=") + str((Sentry3.GetValue(sentry3_vision_e.kVisionAprilTag,sentry_obj_info_e.kHeightValue,i)))), 64, 48, 1)
    oled.show()
    time.sleep(0.1)
    oled.fill_rect(0, (2-1)*16, 128, 16, 0)
    oled.fill_rect(0, (3-1)*16, 128, 16, 0)
    oled.fill_rect(0, (4-1)*16, 128, 16, 0)
    oled.show()
    time.sleep(0.1)
    i = (i + 1)
def Param_Set():
  global my_variable, i, num, flag, param_set
  if (param_set == 1):
    rgb[1] = (0*brightness//9, 0*brightness//9, 255*brightness//9)
    rgb.write()
    param_set = 0
    Sentry3.VisionEnd(sentry3_vision_e.kVisionAprilTag)
    if (flag == 3):
      flag = 0
    if (flag == 0):
      Sentry3.VisionSetMode(sentry3_vision_e.kVisionAprilTag, apriltag_vision_mode_e.kVisionModeFamily36H11)
    if (flag == 1):
      Sentry3.VisionSetMode(sentry3_vision_e.kVisionAprilTag, apriltag_vision_mode_e.kVisionModeFamily25H9)
    if (flag == 2):
      Sentry3.VisionSetMode(sentry3_vision_e.kVisionAprilTag, apriltag_vision_mode_e.kVisionModeFamily16H5)
    # 等待参数生效，此延时不可省略
    time.sleep(1)
    Sentry3.VisionBegin(sentry3_vision_e.kVisionAprilTag)
    rgb[1] = (0*brightness//9, 0*brightness//9, 0*brightness//9)
    rgb.write()

# 事件回调函数
def on_button_a_down(_):
  time.sleep_ms(10)
  if button_a.value() == 1: return
  global flag
  global param_set
  param_set = 1
  flag = (flag + 1)


button_a.irq(trigger=Pin.IRQ_FALLING, handler=on_button_a_down)
param_set = 0
flag = 0
num = 0
i = 0
# 等待Sentry3完成初始化，本延时不可省略，避免出现Sentry3初始化尚未完成主控板已经发送指令
time.sleep(2)
I2C()
# 可以识别下拉菜单列出的3种Apriltag，默认36H11格式。如无需更改编码格式，则本代码块可以去掉。
Sentry3.VisionSetMode(sentry3_vision_e.kVisionAprilTag, apriltag_vision_mode_e.kVisionModeFamily36H11)
# Sentry3最多可以同时识别25个Apriltag
Sentry3.VisionBegin(sentry3_vision_e.kVisionAprilTag)
oled.DispChar("Algo: Apriltag", 0, (1-1)*16, 1)
oled.show()
while True:
  Param_Set()
  Result_Display()
